#!/usr/bin/perl
# 该程序用来计算步进电机加速度的查找表
#

use strict;
use warnings;

use Getopt::Long;

# options
my $force = 0;
my $pcb = 1;
my $usage = <<EOF;
usage: $0 [options]

options:
-f --force  force update
-a<n>       pcb version

EOF

GetOptions(
    'force' => \$force,
    'pcb=i' => \$pcb,
)
    or die $usage;

my $timerclk = 120.0e6;         # 定时器T0频率
my $sysclk;                     # PCA0频率
my $microstep = 2;              # 细分数
my $factor = 2 / 16;            # 计算值系数

print $pcb;

my $speed = 10000;              # 最高速度, 0.01 rad/s
my $accel = 30000;              # 加速度, 0.01 rad/s^2
my $decel = 30000;              # 减速度, 0.01 rad/s^2
my $min_speed = 600;            # 最小转速, 0.01 rad/s

my $speed_interval_code = 0;    # 显示表间隔
my $speed_interval = 1 << $speed_interval_code;

my $fspr = 200;                 # 每圈整步步数

my $table_type = "uchar";       # 表类型

#########################################################
# 新设计样机参数
my $x_reduction_ratio = 42 / 15;            # X减速比
my $y_teeth = 15;               # Y齿轮齿数
my $y_pitch = 2.032;            # Y齿距
my $Z_pitch = 2.5;              # Z齿距
#########################################################

my $pi = 3.14159265;
my $spr = $fspr * $microstep;   # 细分后一圈的步数
my $alpha = 2 * $pi / $spr;     # 每步转角, rad

my $config_h = "config.h";
my $h_file_name = "freq_table.h";
my $c_file_name = "freq_table.c";

my $pl_time     = (stat($0))[9];
my $config_time = (stat($config_h))[9];
my $h_time      = (stat($h_file_name))[9];
my $c_time      = (stat($c_file_name))[9];

my $update_c_file = $force || !defined($c_time) || $config_time > $c_time || $pl_time > $c_time;
my $update_h_file = $force || !defined($h_time) || $config_time > $h_time || $pl_time > $h_time;

unless ($update_c_file || $update_h_file) {
    print "Every thing is up to date\n";
    exit;
}

my $pca_freq = $speed / (2 * $pi * 100) * $spr;     # PCA0输出最大频率
my $timer_div = int($timerclk / ($pca_freq * 2));   # T0溢出值

$timer_div = 16;

$timer_div = 256 if ($timer_div > 256);
$timer_div = 1 if ($timer_div < 1);

my $timer_reload = 256 - $timer_div;                # T0 重装值
$sysclk = $timerclk / $timer_div;                   # PCA基频

# 调试数据, 最大/小输出频率和转速
my $max_freq_output = $sysclk / 2;
my $min_freq_output = $sysclk / (2 * 256);
my $max_speed_output = $max_freq_output / $spr * 2 * $pi * 100;
my $min_speed_output = $min_freq_output / $spr * 2 * $pi * 100;

my $c0 = $sysclk * sqrt(2 * $alpha * 100 / $accel);
my $min_delay = $alpha * $sysclk / ($speed / 100);

#
#                    2C[n-1]
#  C[n] = C[n-1] - ----------                 (1)
#                    4n + 1
#
#
#        C[n-1] + C[n]
#  n = -------------------                    (2)
#       4 (C[n-1] - C[n])
#
#
#
#  C[n] = C[0] ( sqrt(n+1) - sqrt(n) )        (3)
#
#
#
#        C[0]      C[n]   2
#  n = (------- - -------)                    (4)
#        2C[n]     2C[0]
#
#

my @counter = ($c0);

# 先按(3)计算, 等C[n-1] - C[n] < 1时, 按(4)计算
for (my $n = 1; ; $n++) {
    my $cn = $c0 * (sqrt($n + 1) - sqrt($n));
    last if ($cn < $min_delay);
    #last if (int($cn) == int($counter[$n-1]));
    push @counter, $cn * $factor;
}

# 按(4)计算
#for (;;) {
#   my $cn = int($counter[$#counter] - 1);
#   last if $cn < $min_delay;

#   my $n = $c0 / (2 * $cn) - $cn / (2 * $c0);
#   $n = int($n * $n);
#   while($#counter < $n) {
#       push @counter, $cn;
#   }
#}

# 对counter的求和, $sum_table[n] = $counter[0] + ... + $counter[n]
my @sum_table = ();
my $sum = 0;

for (my $n = 0; $n < scalar @counter; $n++) {
    $counter[$n] /= 2;
    if ($counter[$n] >= 256) {
        $counter[$n] = 0;
        $sum += 256;
    } elsif ($counter[$n] < 1) {
        $counter[$n] = 1;
        $sum += 1;
    } else {
        $sum += int($counter[$n]);
    }

    push @sum_table, $sum;
}

# 求和表数据类型和打印宽度
my ($sum_type, $sum_width) = $sum > 65535 ? ("long", 8) : ("ushort", 4);

my $min_speed_n = int($min_speed * $min_speed / 100 / (2 * $alpha * $accel));

# 输出C文件代码

my $count = int(scalar @counter / $speed_interval);

# h file
if ($update_h_file) {
    print "$h_file_name\n";
    open(my $h_file, ">", $h_file_name) or die "Can't open $h_file_name:$!";
    print $h_file <<EOF;
#ifndef _FREQ_TABLE_H_
#define _FREQ_TABLE_H_

//
// 由freq.pl自动生成, 请不要修改
//
// 最大输出频率/转速: $max_freq_output / $max_speed_output
// 最小输出频率/转速: $min_freq_output / $min_speed_output
//

#define T0_RELOAD               $timer_reload               // T0重装值
#define MICROSTEP               $microstep                  // 细分步数
#define FSPR                    $fspr                       // 每转整步数
#define SPR                     $spr                        // 转一圈的步数
#define ALPHA                   $alpha                      // 每步角度, rad
#define SPEED_INTERVAL_CODE     $speed_interval_code        // 表间隔, 2^n
#define SPEED_TABLE_SIZE        $count
#define ACCEL                   $accel          // 加速度, 0.01 rad/s^2
#define DECEL                   $decel          // 减速度, 0.01 rad/s^2
#define SPEED                   $speed          // 最高速度, 0.01 rad/s
#define MIN_SPEED_INDEX         $min_speed_n                // 最小速度输出索引值
#define FREQ_TABLE(n)           freq_table[(n) >> SPEED_INTERVAL_CODE]
#define SUM_TABLE(n)            sum_table[(n) >> SPEED_INTERVAL_CODE]
#define TIME_TABLE(n)           (freq_table[(n) >> SPEED_INTERVAL_CODE] ? \\
                                 freq_table[(n) >> SPEED_INTERVAL_CODE] : 256)

extern $table_type code freq_table[SPEED_TABLE_SIZE];
extern $sum_type code sum_table[SPEED_TABLE_SIZE];

#endif // _FREQ_TABLE_H_
EOF
    close($h_file);
}

# c file
if ($update_c_file) {
    print "$c_file_name\n";
    open(my $c_file, ">", $c_file_name) or die "Can't open $c_file_name:$!";
    print $c_file <<EOF;
#include "device.h"
#include "$h_file_name"

$table_type code freq_table[SPEED_TABLE_SIZE] =
{
EOF

# 定时器初值表
print $c_file "\t";
for(my $s = 0; $s < $count; $s++) {
    printf $c_file "0x%02X, ", @counter[$s * $speed_interval];
    if ($s % 8 == 7) {
        print $c_file "\n\t";
    }
}

print $c_file <<EOF;

};

$sum_type code sum_table[SPEED_TABLE_SIZE] =
{
EOF

    # 定时器求和表
    print $c_file "\t";
    for(my $s = 0; $s < $count; $s++) {
        printf $c_file "0x%0${sum_width}X, ", @sum_table[$s * $speed_interval];
        if ($s % 8 == 7) {
            print $c_file "\n\t";
        }
    }

    print $c_file "\n};\n";

    print $c_file "\n/*\n";
    print $c_file "[ID ]        Hz       rps       rpm     speed\n";

    for(my $s = 256; $s > 0; --$s) {
        my $freq = $sysclk / 2 / $s * $factor;
        my $rps = $freq / $spr;
        my $rpm = $rps * 60;
        my $speed = $rps * 2 * $pi * 100;
        printf $c_file "[%3d]%10.1f%10.2f%10.1f%10.1f\n", $s, $freq, $rps, $rpm, $speed;
    }

    print $c_file "*/\n";

    close($c_file);
}
